#include <SoftwareSerial.h>
#include <ax12.h>
#include <string.h>
#include <ctype.h>

 int ledPin = 13;
 int byteGPS=-1;
 char linea[30] = "";
 char comando[3] = "$A";
 int cont=0;
 int arm_count=0;
 int conta=0;
 int indices[13];
 int gen_counter = 0;

//SoftwareSerial mySerial = SoftwareSerial(2, 3);
AX12 motor_s;
AX12 motor_e;

void setup() {
  motor_s = AX12();
  motor_e = AX12();
  //Serial.begin (115200);  // inicializa el Serial a 115,2 Kb/s
  Serial.begin (9600);  // inicializa el Serial a 9,6 Kb/s
  AX12::init (1000000);   // inicializa los AX12 a 1 Mb/s
  
   Serial.println("Initilize buffer");
   for (int i=0;i<30;i++){       // Initialize a buffer for received data
     linea[i]=' ';
   }
  Serial.println("Done");


  byte detect[2];          
  byte num = AX12::autoDetect (detect, 2); // detección de IDs

  Serial.print (" deteccion: ");
  Serial.println (num, DEC);

  motor_s.id = detect[0]; // asigna las ID detectadas a los motores definidos previamente
  motor_e.id = detect[1];
  motor_s.SRL = RETURN_ALL;
  motor_e.SRL = RETURN_ALL;
  
  Serial.print (" ID detectado: ");
  Serial.println (detect[0], DEC);
  Serial.print (" ID detectado: ");
  Serial.println (detect[1], DEC);
  Serial.print (" delay: ");
  AX12info test1 = motor_s.readInfo (RETURN_DELAY_TIME);
  AX12info test2 = motor_e.readInfo (RETURN_DELAY_TIME);
  Serial.println (test1.value, DEC);
  Serial.print (" error lectura: ");
  Serial.println (test1.error, DEC);
  
  int pos_s = motor_s.getPos();
  int pos_e = motor_e.getPos();
  Serial.print("Actual Shoulder angle is: ");
  Serial.println(pos_s, DEC);
  Serial.print("Actual Elbow angle is: ");
  Serial.println(pos_e, DEC);
 
}

void loop() {
  
      int myindex =0;
      int value[3];
      digitalWrite(ledPin, HIGH);
      
      //while (conta < 22){
         while (Serial.available()) {  
         //byteGPS = Serial.read();
         linea[conta]=Serial.read();
         conta++;        // If there is serial port data, it is put in the buffer    
         Serial.print(linea[conta-1], BYTE);
         }
      //}
        if (linea[conta-1]==59) {
           cont = 0;
           arm_count = 0;
           for (int i=0;i<3;i++){     // Verifies if the received command starts with $A 
             if (linea[i]== comando[i]){
              arm_count++;
             }
           }
           if (arm_count == 2) {             // Reads data from the array
              Serial.println("Is Actuator command");         
              for (int i=2;i<conta+1;i++){
                if (linea[i]=='s'){    // check for the position of the  "," separator or the new update is looking for the command and not commas
                   gen_counter = i;
                   shoulder_angle();
                   i = i + 3;
                   //break;
                   //indices[cont++]=i;
                }
                else if (linea[i]== 'e'){
                   gen_counter = i; 
                   elbow_angle();
                   i = i + 3; 
                   //break;
                }
                else if (linea[i]== 'v'){
                   gen_counter = i;
                   vertical_position();
                   i = i + 3; 
                   //break;
                }
                else if (linea[i]== 'g'){
                   gen_counter = i;
                   gripper_angle(); 
                   i = i + 3;
                   //break;
                }
                else if (linea[i] == ';'){
                   Serial.println("End of transimission, waiting for next command");
                   conta=0;       // Reset the buffer
                   for (int j=0;j<30;j++){    //  
                   linea[j]=' ';
                }
                }
                else {
                   Serial.println("There is no command");
                   conta=0;       // Reset the buffer
                   for (int j=0;j<30;j++){    //  
                   linea[j]=' ';
                }
                }
                //if (linea[i]==';'){    // ... and the ";"
                //   indices[6]=i;
                //   cont++;
                //}
              }

              //Serial.println("");      // ... and write to the serial port
              //Serial.println("");
              //Serial.println("---------------");
              //for (int i=0;i<4;i++){
                //switch(i){
                  //case 0 :shoulder_angle(); break;                //go to shoulder function and move servo
                  //case 1 :elbow_angle(); break;                   //go to elbow function and move servo
                  //case 2 :vertical_position(); break;             //go to vertical function and move servo
                  //case 3 :gripper_angle(); break;                 //go to gripper function and move servo            
                //}
           
                Serial.println("");
              //}
           }
           conta=0;       // Reset the buffer
           for (int i=0;i<30;i++){    //  
           linea[i]=' ';
           }    
        }
        //else {
        //   conta=0;       // Reset the buffer
        //   for (int i=0;i<30;i++){    //  
        //   linea[i]=' ';
        //}
        //}
}



void shoulder_angle(){
  //int myindex2;
  //Serial.println("I am shoulder ");
           
           //for (int j=indices[0];j<(indices[1]-1);j++){
           //myindex2 = indices[0] +2;
           //}       
    int angle_s = atoi(&linea[gen_counter +1]);
    motor_s.setVel (200);

    motor_s.setPos (angle_s);
    //delay(100);
    int pos = motor_s.getPos(); 
    if (pos == angle_s){
      return;
    }
    else {
      motor_s.setPos (angle_s);
    }
    //int vel = motor.getSpeed(); 
    //int load = motor.getLoad();
    //delay(100);
    //Serial.println (" ");
    //Serial.print (" Shoulder: ");
    //Serial.println (pos, DEC);
    //Serial.print (" velocidad: ");
    //Serial.println (vel, DEC);
    //Serial.print (" carga: ");
    //Serial.println (load, DEC);
        
    //delay (300);
}

void elbow_angle(){
  //int myindex2;
  //Serial.println("I am elbow ");
           
           //for (int j=indices[1];j<(indices[2]-1);j++){
           //  myindex2 = indices[1] +2;
           //}
  int angle_e = atoi(&linea[gen_counter +1]);
    motor_e.setVel (200);

    motor_e.setPos (angle_e);
    //delay(100);
    int pos_2 = motor_e.getPos(); 
    if (pos_2 == angle_e){
       return;
    }
    else {
      motor_e.setPos (angle_e);
    }    
  //int pWrist = atoi(&linea[myindex2]);
  //Serial.print("And my value is: ");
  //Serial.println(pWrist);
  //delay (100);
  //Serial.println (" ");
  //Serial.print("Elbow: ");
  //Serial.println(pos_2, DEC);
}

void vertical_position(){
  //int myindex2;
  //Serial.println("I am vertical ");
           
           //for (int j=indices[2];j<(indices[3]-1);j++){
           //  myindex2 = indices[2] +2;
           //}
           
  //int pWrist = atoi(&linea[myindex2]);
  //Serial.print("And my value is: ");
  //Serial.println(pWrist);
  //delay (300);
  int angle = atoi(&linea[gen_counter +1]);
  //Serial.print("And my value is: ");
  //Serial.println(angle);
}

void gripper_angle(){
  //int myindex2;
  //Serial.println("I am gripper ");
           
           //for (int j=indices[3];j<(indices[4]-1);j++){
           //  myindex2 = indices[3] +2;
           //}
           
  //int pWrist = atoi(&linea[myindex2]);
  //Serial.print("And my value is: ");
  //Serial.println(pWrist);
  //delay (300);
  int angle = atoi(&linea[gen_counter +1]);
  //Serial.print("And my value is: ");
  //Serial.println(angle);
}
